#include "defines.h"
#include "kernel.h"
#include "kozos.h"
#include "softvec.h"
#include "syscall.h"
#include "lib.h"
#include "bitsearch.h"

void dispatch(uint32 sp);

static struct _readyque {
    kz_thread *head;
    kz_thread *tail;
} readyque[PRIORITY_NUM];

kz_thread *current;
kz_thread threads[THREAD_NUM];
static kz_handler_t handlers[SOFTVEC_TYPE_NUM];
int in_thread;
static uint32 readyque_bitmap;
static kz_thread_id_t freethreads[THREAD_NUM];
static kz_thread_id_t freethread_num;

static int putready(kz_thread *p)
{
    ASSERT(p, "putready: p is NULL");
    ASSERT(!(p->flags & THREAD_READY), "putready: already in readyque");

    if (readyque[p->priority].tail) {
        readyque[p->priority].tail->next = p;
    } else {
        readyque[p->priority].head = p;
    }
    readyque[p->priority].tail = p;
    readyque_bitmap |= bit32(p->priority);
    p->flags |= THREAD_READY;

    return 0;
}

static void thread_init(kz_thread *thp)
{
    thp->init.func(thp->init.argc, thp->init.argv);
    kz_exit();
    ASSERT(0, "thread_init: NEVERREACH");
}

static kz_thread *make_thread(kz_func_t func, char *name, int priority, unsigned stacksize, int argc, char *argv)
{
    kz_thread_id_t id;
    kz_thread *thp;
    uint32 *sp;

    if (freethread_num) {
        id = freethreads[--freethread_num];
        thp = &threads[id];
        ASSERT(thp->id == id, "make_thread: id error");
    } else {
        return NULL;
    }

    ASSERT(0 <= priority && priority < PRIORITY_NUM, "make_thread: Bad priority");
    thp->priority = priority;

    ASSERT(strlen(name) <= THREAD_NAME_SIZE, "make_thread: too long name");
    strcpy(thp->name, name);

    thp->flags = THREAD_ALIVE;
    thp->next = NULL;

    thp->stack = (char *)kzmem_alloc(stacksize) + stacksize;
    thp->stacksize = stacksize;

    sp = (uint32 *)thp->stack;
    *--sp = (uint32)thread_init | (uint32)(priority ? 0 : 0xc0) << 24;
    /* ER6-ER0 */
    *--sp = 0;
    *--sp = 0;
    *--sp = 0;
    *--sp = 0;
    *--sp = 0;
    *--sp = 0;
    *--sp = (uint32)thp; /* thread_initの引数 */

    thp->context.sp = (uint32)sp;

    thp->init.func = func;
    thp->init.argc = argc;
    thp->init.argv = argv;

    return thp;
}

kz_thread_id_t sys_run(kz_func_t func, char *name, int priority, unsigned stacksize, int argc, char *argv)
{
    kz_thread *thp;
    thp = make_thread(func, name, priority, stacksize, argc, argv);
    ASSERT(thp, "make_thread: can't make thread");
    putready(thp);
    return thp->id;
}

static int sys_exit(void)
{
    ASSERT(current, "sys_exit: current is NULL");
    kzmem_free(current->stack - current->stacksize);
    current->flags = 0; // THREAD_ALIVEを落とす
    freethreads[freethread_num++] = current->id;
    current = NULL;
    return 0;
}

static int sys_wait(uint32 tick)
{
    if (tick) {
        set_alarm(current, tick);
        current = NULL;
    }
    return 0;
}

static int sys_sleep(void)
{
    current = NULL;
    return 0;
}

void thread_wakeup(kz_thread *thp)
{
    ASSERT(thp, "thread_wakeup: thp is NULL");
    ASSERT(thp != current && !(thp->flags & THREAD_READY), "thread_wakeup: thread isn't in sleep");

    if (thp->flags & THREAD_WAITMSG) { // recv(a)を起こす
        cancelreceiver(thp->syscall.param->un.recv.id);
    } else { // sleep・waitを起こす
        if (thp->flags & THREAD_WAITALARM) {
            thp->syscall.param->un.wait.ret = -1;
        }
    }

    cancel_alarm(thp);
    putready(thp);
}

int sys_wakeup(kz_thread_id_t id)
{
    kz_thread *thp;

    ASSERT(0 <= id && id < THREAD_NUM, "sys_wakeup: invalid ID");

    thp = &threads[id];
    if (thp == current || thp->flags & THREAD_READY) {
        return -1;
    } else {
        thread_wakeup(thp);
        return 0;
    }
}

static kz_thread_id_t sys_getid(void)
{
    return current->id;
}

static int sys_chpri(int priority)
{
    int old = current->priority;
    ASSERT(0 <= priority && priority < PRIORITY_NUM, "sys_chpri: Bad priority");
    if (priority >= 0)
        current->priority = priority;
    return old;
}

void *sys_kmalloc(int size)
{
    return kzmem_alloc(size);
}

int sys_kmfree(void *p)
{
    kzmem_free(p);
    return 0;
}

int sys_send(kz_msgbox_id_t id, int size, char *p, int priority, int maxnum, kz_thread *sender)
{
    kz_thread *receiver = getreceiver(id);

    if (receiver) {
        sendreceiver(id, sender, size, p);
        cancel_alarm(receiver);
        putready(receiver);
        return 0;
    } else {
        return pushmsg(id, size, p, priority, maxnum, sender);
    }
}

static kz_thread_id_t sys_recv(kz_msgbox_id_t id, int *sizep, char **pp)
{
    int priority = msg_arrived(id);
    if (priority != -1) {
        return recvmsg(id, priority, current); // current->syscall.paramを通して書込む
    } else {
        setreceiver(id, current);
        current = NULL;
        return THREAD_ID_ERR;
    }
}

static kz_thread_id_t sys_recva(kz_msgbox_id_t id, int *sizep, char **pp, uint32 tick)
{
    int priority = msg_arrived(id);
    if (priority != -1) {
        return recvmsg(id, priority, current);
    } else if (tick == 0) {
        return THREAD_ID_ERR;
    } else {
        setreceiver(id, current);
        set_alarm(current, tick);
        current = NULL;
        return THREAD_ID_ERR;
    }
}

kz_thread_id_t sys_recvi(kz_msgbox_id_t id, int *sizep, char **pp)
{
    int priority = msg_arrived(id);
    if (priority) {
        return popmsg(id, priority, sizep, pp);
    } else {
        return THREAD_ID_ERR;
    }
}

static void thread_intr(softvec_type_t type, unsigned long sp);

int sys_setintr(softvec_type_t type, kz_handler_t handler)
{
    ASSERT(0 <= type && type < SOFTVEC_TYPE_NUM, "sys_setintr: Bad type");
    softvec_setintr(type, thread_intr);
    handlers[type] = handler;
    return 0;
}

static void default_intr(void);

int sys_resetintr(softvec_type_t type)
{
    return sys_setintr(type, default_intr);
}

kz_msgbox_id_t sys_getmsgbox()
{
    return msgbox_get();
}

int sys_freemsgbox(kz_msgbox_id_t id)
{
    msgbox_free(id);
    return 0;
}

char *sys_getthreads()
{
    char *ret;
    kz_threadinfo_t *p;
    int size = 0;
    kz_thread *thp;

    ret = kz_kmalloc(sizeof(kz_threadinfo_t) * THREAD_NUM);
    p = (kz_threadinfo_t *)ret;

    for (thp = threads; thp < threads + THREAD_NUM; thp++) {
        if (thp->flags & THREAD_ALIVE) {
            memcpy(p, thp, sizeof(p->info));
            p->is_current = thp == current;
            p++;
            size++;
        }
    }
    if (size < THREAD_NUM) {
        p->info.id = THREAD_ID_NULL;
    }

    return ret;
}

static void resource_thread(kz_resourceinfo_t *infop)
{
    infop->type = RESOURCE_THREAD;
    infop->num = THREAD_NUM;
    infop->used = THREAD_NUM - freethread_num;
}

char *sys_resource()
{
    char *ret;
    kz_resourceinfo_t *p;

    ret = kz_kmalloc(sizeof(kz_resourceinfo_t) * (MEMORY_AREA_NUM + 3));
    p = (kz_resourceinfo_t *)ret;

    p->type = RESOURCE_HEADER;
    p->size = MEMORY_AREA_NUM + 3;
    p++;

    resource_thread(p++);

    resource_memory(p);
    p += MEMORY_AREA_NUM;

    resource_msgbox(p++);

    return ret;
}

uint32 sys_systime()
{
    return get_systime();
}

static void call_run(kz_syscall_param_t *p)
{
    p->un.run.ret = sys_run(p->un.run.func, p->un.run.name, p->un.run.priority,
                                p->un.run.stacksize, p->un.run.argc, p->un.run.argv);
}

static void call_exit(kz_syscall_param_t *p)
{
    sys_exit();
}

static void call_wait(kz_syscall_param_t *p)
{
    p->un.wait.ret = sys_wait(p->un.wait.tick);
}

static void call_sleep(kz_syscall_param_t *p)
{
    p->un.sleep.ret = sys_sleep();
}

static void call_wakeup(kz_syscall_param_t *p)
{
    p->un.wakeup.ret = sys_wakeup(p->un.wakeup.id);
}

static void call_getid(kz_syscall_param_t *p)
{
    p->un.getid.ret = sys_getid();
}

static void call_chpri(kz_syscall_param_t *p)
{
    p->un.chpri.ret = sys_chpri(p->un.chpri.priority);
}

static void call_kmalloc(kz_syscall_param_t *p)
{
    p->un.kmalloc.ret = sys_kmalloc(p->un.kmalloc.size);
}

static void call_kmfree(kz_syscall_param_t *p)
{
    p->un.kmfree.ret = sys_kmfree(p->un.kmfree.p);
}

static void call_send(kz_syscall_param_t *p)
{
    p->un.send.ret = sys_send(p->un.send.id, p->un.send.size, p->un.send.p, p->un.send.priority, p->un.send.maxnum, current);
}

static void call_recv(kz_syscall_param_t *p)
{
    p->un.recv.ret = sys_recv(p->un.recv.id, p->un.recv.sizep, p->un.recv.pp);
}

static void call_recva(kz_syscall_param_t *p)
{
    p->un.recv.ret = sys_recva(p->un.recv.id, p->un.recv.sizep, p->un.recv.pp, p->un.recv.tick);
}

static void call_setintr(kz_syscall_param_t *p)
{
    p->un.setintr.ret = sys_setintr(p->un.setintr.type, p->un.setintr.handler);
}

static void call_resetintr(kz_syscall_param_t *p)
{
    p->un.resetintr.ret = sys_resetintr(p->un.resetintr.type);
}

static void call_getmsgbox(kz_syscall_param_t *p)
{
    p->un.getmsgbox.ret = sys_getmsgbox();
}

static void call_freemsgbox(kz_syscall_param_t *p)
{
    p->un.freemsgbox.ret = sys_freemsgbox(p->un.freemsgbox.id);
}

static void call_getthreads(kz_syscall_param_t *p)
{
    p->un.getthreads.ret = sys_getthreads();
}

static void call_resource(kz_syscall_param_t *p)
{
    p->un.resource.ret = sys_resource();
}

static void call_systime(kz_syscall_param_t *p)
{
    p->un.systime.ret = sys_systime();
}

static void (*call_functions[])(kz_syscall_param_t *) = {
    call_run,
    call_exit,
    call_wait,
    call_sleep,
    call_wakeup,
    call_getid,
    call_chpri,
    call_kmalloc,
    call_kmfree,
    call_send,
    call_recv,
    call_recva,
    call_setintr,
    call_resetintr,
    call_getmsgbox,
    call_freemsgbox,
    call_getthreads,
    call_resource,
    call_systime,
};

static void schedule(void)
{
    int priority = bitsearch(readyque_bitmap);
    ASSERT(priority != -1, "schedule: no threads");
    struct _readyque *p = &readyque[priority];
    ASSERT(p->head, "schedule: readyque error");

    current = p->head;
    // レディキューから外す。
    p->head = current->next;
    if (!p->head) {
        p->tail = NULL;
        readyque_bitmap &= ~bit32(priority);
    }
    current->flags &= ~THREAD_READY;
    current->next = NULL;
}

static void syscall_intr(void)
{
    call_functions[current->syscall.type](current->syscall.param);
}

/* OSの割込み処理の入口 */
static void thread_intr(softvec_type_t type, unsigned long sp)
{
    current->context.sp = sp;
    in_thread = 0;

    if (handlers[type])
        handlers[type]();

    if (current) { /* 戻す必要がない場合current = NULLになっている */
        putready(current);
        current = NULL;
    }

    schedule();

    in_thread = 1;
    dispatch(current->context.sp);
}

static void default_intr()
{
    kz_sysdown("unused intr");
}

static void intr_init()
{
    softvec_type_t type;
    for (type = 0; type < SOFTVEC_TYPE_NUM; type++)
        sys_resetintr(type);
}

// ライブラリ関数

void kz_start(kz_func_t func, char *name, int priority, unsigned stacksize, int argc, char *argv)
{
    kz_thread_id_t i;
    ASSERT(memeq(readyque, 0, sizeof(readyque)), "BSS isn't cleared");
    ASSERT(memeq(threads, 0, sizeof(threads)), "BSS isn't cleared");
    readyque_bitmap = 0;
    for (i = 0; i < THREAD_NUM; i++) {
        threads[i].id = i;
        freethreads[i] = THREAD_NUM - 1 - i;
    }
    freethread_num = THREAD_NUM;

    intr_init();
    sys_setintr(SOFTVEC_TYPE_TRAP0, syscall_intr);

    kzmem_init();
    ipc_init();
    timer_init();

    current = make_thread(func, name, priority, stacksize, argc, argv); /* レディキューにあってはいけない */
    in_thread = 1;
    dispatch(current->context.sp);
}

void kz_sysdown(const char *message)
{
    INTR_DISABLE;
    puts_k("SYSTEM ERROR! (");
    if (message) {
        puts_k(message);
    }
    puts_k(")\n");
    kz_reset();
}

void kz_reset() // 内部リセット。WDT使用
{
    INTR_DISABLE;
    *((volatile uint16 *)0xffff8c) = 0xa567;
    while (1)
        ;
}
